/*
 * mpu6050.c
 *
 *  Created on: 2017年7月17日
 *      Author: Administrator
 */


#include "r_cg_cgc.h"
#include "r_cg_riic.h"
#include "r_cg_macrodriver.h"
#include "delay.h"
#include "mpu6050.h"
#include "stdlib.h"


uint8_t Mpu_Init(void)
{
	uint8_t res;
	Mpu_WR_Reg(MPU_PWR_MGMT1_REG,0x80);			//复位MPU6050
	Delay_ms(10);
	Mpu_WR_Reg(MPU_PWR_MGMT1_REG,0x00);			//唤醒MPU6050
	Mpu_WR_Reg(MPU_GYRO_CFG_REG,3<<3);						//陀螺仪传感器,±2000dps
	Mpu_WR_Reg(MPU_ACCEL_CFG_REG,1<<3);						//加速度传感器,±4g
	Mpu_WR_Reg(MPU_SAMPLE_RATE_REG,19);
	Mpu_WR_Reg(MPU_CFG_REG,4);							//设置采样率50Hz

	Mpu_WR_Reg(MPU_INT_EN_REG,0X00);      	   //关闭所有中断
	Mpu_WR_Reg(MPU_USER_CTRL_REG,0X00);			//I2C主模式关闭
	Mpu_WR_Reg(MPU_FIFO_EN_REG,0X00);			//关闭FIFO
	Mpu_WR_Reg(MPU_INTBP_CFG_REG,0X80);			//INT引脚低电平有效
	res=Mpu_RE_Reg(MPU_DEVICE_ID_REG);

	if(res==MPU_ADDR)//器件ID正确
	{
		Mpu_WR_Reg(MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
		Mpu_WR_Reg(MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
		Mpu_WR_Reg(MPU_SAMPLE_RATE_REG,19);
	  Mpu_WR_Reg(MPU_CFG_REG,4);							//设置采样率为50Hz
 	}
	else
		return 1;
	return 0;
}

MPUAxis Mpu_acceleration(void)
{
	 MPUAxis	mpuaxis;
	 uint8_t mpu_buf[6],i = 0;
	 uint8_t addr_axyz[6]= {0x3b,0x3c,0x3d,0x3e,0x3f,0x40};   //XYZ的加速度
	 uint8_t addr_gxyz[6]= {0x43,0x44,0x45,0x46,0x47,0x48};   //XYZ的陀螺仪
	 short acc_x,acc_y,acc_z;
	 short ang_x,ang_y,ang_z;

	 short ggx,ggy,ggz;
	 int  aax,aay,aaz;
	 for(i=0;i<6;i++)
	 {
		 mpu_buf[i]=Mpu_RE_Reg(addr_axyz[i]);
		 User_Delay(200);
	 }

	 acc_x=((uint16_t)mpu_buf[0]<<8)|mpu_buf[1];
	 acc_y=((uint16_t)mpu_buf[2]<<8)|mpu_buf[3];
	 acc_z=((uint16_t)mpu_buf[4]<<8)|mpu_buf[5];

	  for(i=0;i<6;i++)
	  {
		   mpu_buf[i]=Mpu_RE_Reg(addr_gxyz[i]);
		   User_Delay(200);
	  }
	  ang_x=((uint16_t)mpu_buf[0]<<8)|mpu_buf[1];
	  ang_y=((uint16_t)mpu_buf[2]<<8)|mpu_buf[3];
	  ang_z=((uint16_t)mpu_buf[4]<<8)|mpu_buf[5];

	  mpuaxis.AccX = acc_x;
	  mpuaxis.AccY = acc_y;
	  mpuaxis.AccZ = acc_z;
	  mpuaxis.GyroX = ang_x;
	  mpuaxis.GyroY = ang_y;
	  mpuaxis.GyroZ = ang_z;

//	  ggx=ang_x*0.05;
//	  ggy=ang_y*0.05;
//	  ggz=ang_z*0.05;
//	  aax=acc_x*4*9.7936/32768*10;
//	  aay=acc_y*4*9.7936/32768*10;
//	  aaz=acc_z*4*9.7936/32768*10;
		return	mpuaxis;
}

void Mpu_WR_Reg(uint8_t Addr,uint8_t Dat)
{
	uint8_t Command[2];
	Command[0] = Addr;
	Command[1] = Dat;
	R_RIIC0_Master_Send(0x68,Command,2);
	User_Delay(200);
}
uint8_t Mpu_RE_Reg(uint8_t Addr)
{
	uint8_t result;
	R_RIIC0_Master_Send(0x68,&Addr,1);
	User_Delay(100);
	R_RIIC0_Master_Receive(0x68,&result,1);
	User_Delay(100);
	return result;

}



